#include "robotModel.h"
#include <stdio.h>
#include <iostream>
void copyData(int * dst,int*src,int len,float k)
{
    for(int i=0;i<len;i++)
        dst[i]=src[i]*k;
}

void copyData(int ** dst,int** src,int len,int hight,float k)
{
    int *pdst,*psrc;
    for(int j=0;j<hight;j++)
    {
        pdst=*(dst+20*j);psrc=*(src+20*j);
        for(int i=0;i<len;i++)
            pdst[i]=psrc[i]*k;
    }
}
void copyDataBlock(int * dst,int* src,int len,int hight,float k)
{
    int *pdst,*psrc;
    for(int j=0;j<hight;j++)
    {
        pdst=(dst+20*j);psrc=(src+20*j);
        for(int i=0;i<len;i++)
            pdst[i]=psrc[i]*k;
    }
}
robotModel::robotModel()
{
    //ctor
    m_array=Eigen::VectorXi(20);
    m_data=m_array.data();


}

robotModel::~robotModel()
{
    //dtor
}
void robotModel::setLeg(double rotateX,double rotateY,double rotateZ,double len,int leg)
{
    //record
     if(leg==left)
    {
        info.left_leg[0]=rotateX;
        info.left_leg[1]=rotateY;
        info.left_leg[2]=rotateZ;
        info.left_leg[3]=len;
    }
    if(leg==right)
    {
        info.right_leg[0]=rotateX;
        info.right_leg[1]=rotateY;
        info.right_leg[2]=rotateZ;
        info.right_leg[3]=len;
    }

    int t_data[4];
    if(leg==1)
    {
        rotateX=-rotateX;
        rotateZ=-rotateZ;
    }

    double t_a=-acos(len/leg_length/2);
    t_data[0]=(t_a+rotateY)/r_p_s;
    t_data[3]=(rotateY-t_a)/r_p_s;
    t_data[2]=rotateZ/r_p_s;
    t_data[1]=rotateX/r_p_s;

    if(leg==left)
        copyData(m_data+8,t_data,4,1);
    else
    if(leg==right)
        copyData(m_data+1,t_data,4,(float)-1.0);
    else
    {
    ;
    }

    updataParam(leg);
}
void robotModel::setFoot(double rotateX,double rotateY ,int leg)
{
    int t_data[2];
    if(leg==right)
        rotateY=-rotateY;

    t_data[1]=-rotateX/r_p_s;
    t_data[0]=rotateY/r_p_s;

    if(leg==left)
        copyData(m_data+12,t_data,2);
    if(leg==right)
        copyData(m_data+5,t_data,2);
    updataParam(leg);
    //std::cout<<m_array;
}
void robotModel::modifyData(int *d)
{
    int i=0;
    while(i++<20)
        m_data[i-1]=d[i-1];
}
void robotModel::getData(int *d)
{
    int i=0;
    while(i++<20)
        d[i-1]=m_data[i-1];
}
void robotModel::showParam(int leg)
{

    if(leg==left)
    {
        printf("rx,ry,rz,len:%.2f,%.2f,%.2f,%.2f\n",info.left_leg[0],info.left_leg[1],info.left_leg[2],info.left_leg[3]);
        printf("rx,ry:%.2f,%.2f\n ",info.left_foot[0],info.left_foot[1]);
    }
    if(leg==right)
    {
        printf("rx,ry,rz,len:%.2f,%.2f,%.2f,%.2f\n",info.right_leg[0],info.right_leg[1],info.right_leg[2],info.right_leg[3]);
        printf("rx,ry:%.2f,%.2f\n ",info.right_foot[0],info.right_foot[1]);
    }

}
void robotModel::updataParam(int leg)
{
    double rotateX,rotateY,rotateZ,len;
    double h=0;
    int t_data[4];
    if(leg==1)
        copyData(t_data,m_data+8,4,1);
    else if(leg==2)
        copyData(t_data,m_data+1,4,-1);
    else
    {
    ;
    }
    rotateX=t_data[1]*r_p_s;
    rotateZ=t_data[2]*r_p_s;
    rotateY=(t_data[0]+t_data[3])/2*r_p_s;
    len=cos((t_data[0]-t_data[3])/2*r_p_s)*leg_length;
    h=leg_length*(cos(t_data[0]*r_p_s)+cos(t_data[3]*r_p_s));
    //printf("angle:%.2f ,%.2f ,%.2f len:%.2f h:%.2f\n",rotateX,rotateY,rotateZ,len,h);

    int e=0;
    if(leg==left)
    {
        info.left_leg[0]=-rotateX;
        info.left_leg[1]=rotateY;
        info.left_leg[2]=-rotateZ;
        info.left_leg[3]=len;
        info.left_foot[0]=-m_data[13]*r_p_s;
        info.left_foot[1]=m_data[12]*r_p_s;

        e=12;
    }
    if(leg==right)
    {
        info.right_leg[0]=rotateX;
        info.right_leg[1]=rotateY;
        info.right_leg[2]=rotateZ;
        info.right_leg[3]=len;
        info.right_foot[0]=-m_data[6]*r_p_s;
        info.right_foot[1]=-m_data[5]*r_p_s;

        e=5;
    }
     //printf("angle:%.2f ,%.2f\n",r_p_s*m_data[e+1],r_p_s*m_data[e]);
}
